mode = 1;
test_arr = ARR_Kinematic_Model(0,5,pi/6,-pi/6,mode);
test_path = Path(200);
test_trp = Point();
test_controller = ARRK_PP_Controller(test_arr.L_f,test_arr.L_r);
test_controller_1 = Normal_PP_Controller(test_arr.L_f,test_arr.L_r);
test_trp.x = 10;
test_trp.y = 20;
test_trp.yaw = pi/3;
turn_a = 0;
t_rate = pi/6;
test_path.Generating_Cycle_Path(test_trp,turn_a,mode,t_rate);
% test_angle_rate = test_controller.Control_Process(test_arr,test_trp,0.1);
% fprintf('out expangle %f angle rate %f\n',test_controller.ExpAng ,test_angle_rate);
axis([0 30 0 30]);
hold on;
for i = 1:test_path.Traject_Size
    scatter(test_path.Trajectory(1,i).x,test_path.Trajectory(1,i).y,'red');
    hold on;
%     pause(0.001);
end

Closest_Index = test_path.Find_Closest_Point(1,test_arr.P_f,test_arr.P_r,mode);
sc_f = scatter(test_arr.P_f.x,test_arr.P_f.y,'blue');
hold on;
sc_m = scatter(test_arr.P_m.x,test_arr.P_m.y,'blue','.');
hold on;
sc_r = scatter(test_arr.P_r.x,test_arr.P_r.y,'green');
hold on;
l_f = line([test_arr.P_f.x,test_arr.P_m.x],[test_arr.P_f.y,test_arr.P_m.y],'Color','black');
hold on;
l_r = line([test_arr.P_m.x,test_arr.P_r.x],[test_arr.P_m.y,test_arr.P_r.y],'Color','black');
hold on;
trpv = scatter(test_path.Trajectory(1,Closest_Index).x,test_path.Trajectory(1,Closest_Index).y,'red','*');
hold on;

l_f1 = line([test_arr.P_f_c2.x,test_arr.P_f_c1.x],[test_arr.P_f_c2.y,test_arr.P_f_c1.y],'Color','black');
hold on;
l_f2 = line([test_arr.P_f_c2.x,test_arr.P_f_c3.x],[test_arr.P_f_c2.y,test_arr.P_f_c3.y],'Color','black');
hold on;
l_f3 = line([test_arr.P_f_c3.x,test_arr.P_f_c4.x],[test_arr.P_f_c3.y,test_arr.P_f_c4.y],'Color','black');
hold on;
l_f4 = line([test_arr.P_f_c4.x,test_arr.P_f_c1.x],[test_arr.P_f_c4.y,test_arr.P_f_c1.y],'Color','black');
hold on;

l_r1 = line([test_arr.P_r_c2.x,test_arr.P_r_c1.x],[test_arr.P_r_c2.y,test_arr.P_r_c1.y],'Color','black');
hold on;
l_r2 = line([test_arr.P_r_c2.x,test_arr.P_r_c3.x],[test_arr.P_r_c2.y,test_arr.P_r_c3.y],'Color','black');
hold on;
l_r3 = line([test_arr.P_r_c3.x,test_arr.P_r_c4.x],[test_arr.P_r_c3.y,test_arr.P_r_c4.y],'Color','black');
hold on;
l_r4 = line([test_arr.P_r_c4.x,test_arr.P_r_c1.x],[test_arr.P_r_c4.y,test_arr.P_r_c1.y],'Color','black');
hold on;

for i = 1:10000
    
    Vehicle_fPoint =  test_arr.P_f;
    Vehicle_rPoint =  test_arr.P_r;
    Closest_Index = test_path.Find_Closest_Point(Closest_Index,Vehicle_fPoint,Vehicle_rPoint,mode);
    fprintf('Closest_Index %d\n',Closest_Index);
    Track_Point = test_path.Trajectory(1,Closest_Index);
%     if mod(i,10) == 0
%         test_angle_rate = test_controller.Control_Process(test_arr,Track_Point,0.1,mode);
        test_angle_rate = test_controller_1.Control_Process(test_arr,Track_Point,0.1);
%     else
%         test_angle_rate = 0;
%     end
    test_arr.State_Update(mode*5, 0.1, test_angle_rate);
    set( sc_f, 'XData', test_arr.P_f.x, 'YData', test_arr.P_f.y );
    set( sc_m, 'XData', test_arr.P_m.x, 'YData', test_arr.P_m.y );
    set( sc_r, 'XData', test_arr.P_r.x, 'YData', test_arr.P_r.y );
    set( l_f, 'XData', [test_arr.P_f.x,test_arr.P_m.x], 'YData', [test_arr.P_f.y,test_arr.P_m.y] );
    set( l_r, 'XData', [test_arr.P_m.x,test_arr.P_r.x], 'YData', [test_arr.P_m.y,test_arr.P_r.y] );
    set( trpv, 'XData', test_path.Trajectory(1,Closest_Index).x, 'YData', test_path.Trajectory(1,Closest_Index).y );
    scatter(test_arr.P_f.x,test_arr.P_f.y,'blue','.');
    scatter(test_arr.P_r.x,test_arr.P_r.y,'green','.');
%     scatter(test_arr.P_m.x,test_arr.P_m.y,'black','.');


    set( l_f1, 'XData', [test_arr.P_f_c2.x,test_arr.P_f_c1.x], 'YData', [test_arr.P_f_c2.y,test_arr.P_f_c1.y] );
    set( l_f2, 'XData', [test_arr.P_f_c2.x,test_arr.P_f_c3.x], 'YData', [test_arr.P_f_c2.y,test_arr.P_f_c3.y] );
    set( l_f3, 'XData', [test_arr.P_f_c3.x,test_arr.P_f_c4.x], 'YData', [test_arr.P_f_c3.y,test_arr.P_f_c4.y] );
    set( l_f4, 'XData', [test_arr.P_f_c4.x,test_arr.P_f_c1.x], 'YData', [test_arr.P_f_c4.y,test_arr.P_f_c1.y] );
    
    set( l_r1, 'XData', [test_arr.P_r_c2.x,test_arr.P_r_c1.x], 'YData', [test_arr.P_r_c2.y,test_arr.P_r_c1.y] );
    set( l_r2, 'XData', [test_arr.P_r_c2.x,test_arr.P_r_c3.x], 'YData', [test_arr.P_r_c2.y,test_arr.P_r_c3.y] );
    set( l_r3, 'XData', [test_arr.P_r_c3.x,test_arr.P_r_c4.x], 'YData', [test_arr.P_r_c3.y,test_arr.P_r_c4.y] );
    set( l_r4, 'XData', [test_arr.P_r_c4.x,test_arr.P_r_c1.x], 'YData', [test_arr.P_r_c4.y,test_arr.P_r_c1.y] );
    
    drawnow;
    pause(0.00000001);
end


% scatter(test_path.Trajectory(1,Closest_Index).x,test_path.Trajectory(1,Closest_Index).y,'red');
% 
% scatter(test_arr.P_f.x,test_arr.P_f.y,'blue');
% hold on;
% scatter(test_arr.P_r.x,test_arr.P_r.y,'green');
% hold on;
% scatter(test_arr.P_m.x,test_arr.P_m.y,'black','.');
% hold on;
% scatter(test_trp.x,test_trp.y,'red','*');
% hold on;